/*
 * mox_threads.c
 *
 *  Created on: Jul 12, 2010
 *      Author: jmonk
 */

#include <stdlib.h>
#include <xccompat.h>
#include "mox.h"
#include "mox_threads.h"

#ifndef FLASHER

int thread_create(void *func, chanend mod,
		int *stack, int *const_ptr, int *data_ptr, int i)
{
	unsigned tmp;
	asm("stw r3,%0[0x0]"::"r"(&tmp));
	asm("getr r3, 0x4");
	asm("set t[r3]:r0, r0");
	asm("set t[r3]:r1, r1");
	asm("set t[r3]:r2, r2");
	asm("set t[r3]:r4, r4");
	asm("set t[r3]:r5, r5");
	asm("set t[r3]:r6, r6");
	asm("set t[r3]:r7, r7");
	asm("set t[r3]:r8, r8");
	asm("set t[r3]:r9, r9");
	asm("set t[r3]:r10, r10");
	asm("set t[r3]:r11, r11");

	//threadid in r1
	asm("ldw r1,%0[0]"::"r"(&func));
	asm("ldw r2,%0[0]"::"r"(&mod));
	asm("ldw r4,%0[0]"::"r"(&stack));
	asm("ldw r5,%0[0]"::"r"(&const_ptr));
	asm("ldw r6,%0[0]"::"r"(&data_ptr));

	asm("ldaw r11, cp[0x00]");
	asm("init t[r3]:cp, r11");
	if (i) {
		asm("set cp,r5");
	}

	asm("ldaw r11, dp[0x00]");
	asm("init t[r3]:dp, r11");
	if (i) {
		asm("set dp, r6");
	}

	asm("ldaw r11, sp[0x00]");		//need a better way to do this, causing problems
	asm("init t[r3]:sp, r11");
	asm("set sp,r4");

	asm("ldap r11, 0x4");
	asm("init t[r3]:pc, r11");
	asm("start t[r3]");

	asm("add r10,r2,0x0");
	asm("bla r1");

	asm("ldw r3,%0[0x0]"::"r"(&tmp));
	return 0;
}
inline void thread_exit(void)
{
	asm("freet");
}

unsigned thd_std_start(chanend mox, mox_msg thd_info)
{
	mox_msg *msg;
	unsigned id;

	while (1) {
		msg = mox_comm_msg_recv(mox);
		if (msg && (msg->type == TH_REQ_INFO)) {
			mox_comm_msg_send(mox, thd_info);
			if (msg->content) free(msg->content);
			free(msg);
		} else if (msg && (msg->type == TH_RELEASE)) {
			id = ((unsigned *)msg->content)[0];
			free(msg->content);
			free(msg);
			return id;
		}
	}
}

void thd_comm_msg_send(chanend mox, mox_msg msg)
{
	unsigned tmp;
	while (TH_ACK != mox_chan_in_wd(mox)) mox_chan_out_wd(mox, TH_NACK);
	mox_chan_out_wd(mox, MOX_MSG);

	mox_chan_out_wd(mox, (int)&msg);
	mox_chan_in_wd(mox);
}

mox_msg *thd_comm_msg_recv(chanend mox)
{
	while (MOX_MSG != mox_chan_in_wd(mox)) mox_chan_out_wd(mox, TH_NACK);
	mox_chan_out_wd(mox, TH_ACK);
	return (mox_msg *)mox_chan_in_wd(mox);
}

mox_msg *thd_comm_msg_recv_nblock(chanend mox)
{
	if (MOX_MSG != mox_chan_in_wd(mox)) {
		mox_chan_out_wd(mox, TH_NACK);
		return NULL;
	}
	mox_chan_out_wd(mox, TH_ACK);
	return (mox_msg *)mox_chan_in_wd(mox);
}
#endif
